package ru.spbau.lifeincube.math;

public class Matrix4x4 {
	private double[] data;

	public Matrix4x4() {
		data = new double[16];
		data[0] = 1;
		data[5] = 1;
		data[10] = 1;
		data[15] = 1;
	}

	public Matrix4x4(Quaternion quaternion) {
		this();

		double x = quaternion.getX();
		double y = quaternion.getY();
		double z = quaternion.getZ();
		double w = quaternion.getW();

		data[0] = 1.0 - 2.0 * y * y - 2.0 * z * z;
		data[1] = 2.0 * x * y + 2.0 * w * z;
		data[2] = 2.0 * x * z - 2.0 * w * y;
		data[3] = 0.0;

		data[4] = 2.0 * x * y - 2.0 * w * z;
		data[5] = 1.0 - 2.0 * x * x - 2.0 * z * z;
		data[6] = 2.0 * y * z + 2.0 * w * x;
		data[7] = 0.0;

		data[8] = 2.0 * x * z + 2.0 * w * y;
		data[9] = 2.0 * y * z - 2.0 * w * x;
		data[10] = 1.0 - 2.0 * x * x - 2.0 * y * y;
		data[11] = 0.0;

		data[12] = 0.0;
		data[13] = 0.0;
		data[14] = 0.0;
		data[15] = 1.0;
	}

	public double get(int i, int j){
		if (i >= 0 && i < 4 && j >=0 && j < 4) {
			return data[i*4 + j];
		}
		throw new RuntimeException("arguments should be in [0,3]");
	}

	public void set(int i, int j, double val) {
		data[i*4 + j] = val;
	}

	public void mult(Matrix4x4 mat) {
		int i;
		int k;

		double[] temp = new double[16];

		int row;
		int column;
		for (i = 0; i < 16; i++)
		{
			temp[i] = 0.0;

			row = i % 4;
			column = (i / 4) * 4;

			for (k = 0; k < 4; k++)
			{
				temp[i] += data[row + k * 4] * mat.data[k + column];
			}
		}

		for (i = 0; i < 16; i++)
		{
			data[i] = temp[i];
		}
	}
}
